#ifndef _OBERON7_HARDWARE_HPP_
#define _OBERON7_HARDWARE_HPP_

#include <memory>
#include <string>
#include <vector>

#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/system_interface.hpp"

#include "hardware_interface/types/lifecycle_state_names.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "hardware_interface/visibility_control.h"
#include "rclcpp/macros.hpp"

#include <chrono>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"

#include "oberon7_robot_driver/oberon7.hpp"

using namespace std::chrono_literals;

namespace oberon7_robot_driver
{

  class Oberon7Hardware : public hardware_interface::SystemInterface
  {
  public:
    RCLCPP_SHARED_PTR_DEFINITIONS(Oberon7Hardware)

    HARDWARE_INTERFACE_PUBLIC
    CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override;

    HARDWARE_INTERFACE_PUBLIC
    CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override;

    HARDWARE_INTERFACE_PUBLIC
    std::vector<hardware_interface::StateInterface> export_state_interfaces() override;

    HARDWARE_INTERFACE_PUBLIC
    std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;

    HARDWARE_INTERFACE_PUBLIC
    CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override;

    HARDWARE_INTERFACE_PUBLIC
    CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override;

    HARDWARE_INTERFACE_PUBLIC
    hardware_interface::return_type read() override;

    HARDWARE_INTERFACE_PUBLIC
    hardware_interface::return_type write() override;

    Oberon7Hardware();
    // Oberon7Driver::SharedPtr _robot;
    Oberon7Driver _robot;

  private:
    void writeCommandsToHardware();
    void updateJointsFromHardware();

    // Store the command for the robot
    std::vector<double> hw_commands_;
    std::vector<double> hw_states_position_, hw_states_velocity_, hw_states_effort_;
  };

} // namespace oberon7_robot_driver

#endif // _OBERON7_HARDWARE_HPP_